//
// Created by xuwj in 2022 / 03.
//

#include "iostream"
#include "MathHelper/MathHelper.h"
#include "opencv2/core/core.hpp"

using namespace std;
using namespace CV_Assistant::MathHelper;

const string config_file = "../configs/MathHelper/params_Math.yaml";

int main(int argc, char **argv){

    cv::FileStorage fSettings(config_file,CV_STORAGE_READ);
    if (!fSettings.isOpened()){
        cout<<"\nError with your config file's path!!!"<<endl;
    }


    /**
     * 从旋转矩阵求四元数
     */
    cv::Mat Mat1;
    fSettings["RotationMatrix1"] >> Mat1;

    cout<<Mat1.cols<<"\t"<<Mat1.rows<<endl;
    cout<<"RotaMat:\n"<<Mat1<<endl<<endl;

    Eigen::Quaterniond q(MathHelper::Converter::toMatrix3d(Mat1));
    cout<<"w\t:\n"<<q.w()<<endl;
    cout<<"x\ty\tz:\n"<<q.x()<<"\t"<<q.y()<<"\t"<<q.z()<<"\n";


    /**
     * 从4*4位姿变换矩阵 求 逆变换
     */
//    cv::Mat Mat1;
//    fSettings["TransMatrix1"] >> Mat1;
//    cv::Mat R_inverse = Mat1.rowRange(0,3).colRange(0,3).t();
//    cv::Mat t_inverse = -R_inverse*Mat1.rowRange(0,3).col(3);

//    cv::Mat ans = (cv::Mat_<float>(4,4)<<
//            R_inverse.at<float>(0,0), R_inverse.at<float>(0,1), R_inverse.at<float>(0,2), t_inverse.at<float>(0),
//            R_inverse.at<float>(1,0), R_inverse.at<float>(1,1), R_inverse.at<float>(1,2), t_inverse.at<float>(1),
//            R_inverse.at<float>(2,0), R_inverse.at<float>(2,1), R_inverse.at<float>(2,2), t_inverse.at<float>(2),
//            0.0, 0.0, 0.0, 1.0);
//    cout<<"Inverse pose-transformation matrix:\n"<<ans<<endl;


    return 0;
}
